% TEST FUNCTION
% -------------
% This function converts the angles alpha, beta, gamma back into
% the point x,y,z. 
%
% This function is used just for test purposes to confirm that
% calculated angles are correct.

function ret = a2p( robot, A )
coxa = robot(1,1);%coxa=14.8;
femur= robot(1,2);%femur=45.45; 
tibia= robot(1,3);%tibia=59.29; 
base = robot(1,4);%base=92;     

	gamma=A(1,1);
	alpha=A(1,2);
	beta=A(1,3);

	x1=cos((gamma*pi/180))*coxa;
	y1=sin((gamma*pi/180))*coxa;
	z1=base;
	
	n1=sin(((180-alpha)*pi/180))*femur;
	n2=cos(((180-alpha)*pi/180))*femur;
	x2=cos((gamma*pi/180))*(coxa+n1);
	y2=sin((gamma*pi/180))*(coxa+n1);
	z2=base+n2;

	T=90-(beta-(180-alpha));
	n3=cos((T*pi/180))*tibia;
	n4=sin((T*pi/180))*tibia;
	
	x3=cos((gamma*pi/180))*(coxa+n1+n3);
	y3=sin((gamma*pi/180))*(coxa+n1+n3);
	z3=base+n2-n4;
	
	ret(1,1)=x1;
	ret(1,2)=y1;
	ret(1,3)=z1;
	ret(2,1)=x2;
	ret(2,2)=y2;
	ret(2,3)=z2;
	ret(3,1)=x3;
	ret(3,2)=y3;
	ret(3,3)=z3;
endfunction
